/*
 * "Copyright (c) 2012 Universitat Pompeu Fabra.
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 *
 * IN NO EVENT SHALL UNIVERSITAT POMPEU FABRA BE LIABLE TO ANY PARTY 
 * FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING 
 * OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF POMPEU FABRA
 * HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * UNIVERSITAT POMPEU FABRA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND UNIVERSITAT POMPEU FABRA HAS NO OBLIGATION TO 
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 */

#include "HbcpNetworkPackets.h"
#include "HbcpStatistics.h"

module HbcpForwardingEngineP {

	provides {
		interface HbcpForwarding as Forwarding;
		interface Init;
		interface Receive as RoutingEvent;
	}

	uses {
		interface HbcpLink as LinkSend;
		interface Receive as LinkReceive;
		interface HbcpRouting as Routing;
		interface CC2420Packet;
		interface Packet;
		interface Queue<dataPacket_t*> as DataQueue;
		interface Queue<uint8_t> as SizeQueue;
		interface Pool<dataPacket_t> as DataPool;
	}
}

implementation {

	/**********************QUEUES AND BUFFERS**********************/ 

	bool activeForwardTask;
	
	dataPacket_t aggBuffer;
	bool appBufferFull = FALSE;
	
	bool readyToSend = FALSE;
	bool aggBufferIsEmpty = TRUE;

	bool isTransmissionAllowed;
	
	command error_t Forwarding.cleanQueues(){
		while(!call DataQueue.empty()){
			call DataPool.put(call DataQueue.dequeue());
		}
		while(!call SizeQueue.empty()){
			call SizeQueue.dequeue();
		}
		return SUCCESS;
	}
	
	command error_t Init.init() {
		activeForwardTask = FALSE;
		call Forwarding.cleanQueues();
		return SUCCESS;
	}
	
	command void Forwarding.allowTransmission(bool status) {
		isTransmissionAllowed = status;
	}
	
	command uint8_t Forwarding.getNumPacketsInQueue(){
		return call DataQueue.size();	
	}

	void initAggBuffer() {

		aggBuffer.generalFrame.networkID = HBTP_TYPE;
		aggBuffer.generalFrame.type = DATA_TYPE;
		aggBuffer.generalFrame.source = TOS_NODE_ID;
		aggBuffer.generalFrame.length = (uint8_t)sizeof(dataPacket_t)-DATA_LENGTH;

		aggBuffer.battery = 0x00;
		aggBuffer.hop = call Routing.getCurrentHop();
		aggBuffer.payloadsNum = 0;
		aggBuffer.payloadsSize = 0;
	}

	error_t aggregatePayload(void * payload, uint8_t len) {

		dataPacket_t * newData = payload;
		uint8_t newLength = 0;
		uint8_t aggLength = 0;

		if(aggBufferIsEmpty) {
			initAggBuffer();
		}

		newLength = newData->payloadsSize;

		if(aggBuffer.payloadsNum == 0) {
			aggLength = 0;
		}
		else {
			aggLength = aggBuffer.payloadsSize;
		}

		if((aggBuffer.payloadsNum + newData->payloadsNum) < MAX_AGGREGATION && aggBuffer.generalFrame.length + newLength < DATA_LENGTH) {

			aggBufferIsEmpty = FALSE;
			aggBuffer.payloadsNum += newData->payloadsNum;

			aggBuffer.generalFrame.length += newLength;
			aggBuffer.payloadsSize += newLength;

			memcpy(aggBuffer.payload + aggLength, newData->payload, newLength);

			return SUCCESS;
		}
		readyToSend = TRUE;
		return FAIL;
	}

	task void forwardTask() {

		dataPacket_t * payload;
		uint8_t len;

		if( ! isTransmissionAllowed) {
			activeForwardTask = FALSE;
			return;
		}

		if(readyToSend == TRUE) {
			if(SUCCESS == call LinkSend.send(call Routing.getNextHop(), &aggBuffer,
					aggBuffer.generalFrame.length)) {
				readyToSend = FALSE;
				aggBufferIsEmpty = TRUE;
			}
		}

		atomic {
			if(call DataQueue.empty()) {
				activeForwardTask = FALSE;
				if( ! aggBufferIsEmpty) {
					if(SUCCESS == call LinkSend.send(call Routing.getNextHop(), &aggBuffer,
							aggBuffer.generalFrame.length)) {
						aggBufferIsEmpty = TRUE;
						return;
					}
					post forwardTask();
				}
				return;
			}
		}
		
		payload = call DataQueue.head();
		len = call SizeQueue.head();
		
		if(SUCCESS == aggregatePayload(payload, len)) {
			
			call DataQueue.dequeue();
			call SizeQueue.dequeue();
				
			call DataPool.put(payload);		
		}
		post forwardTask();
	}

	void queueForwardingPacket(message_t * msg, void * payload, uint8_t len) {
		
		dataPacket_t * packet;

		if(call DataQueue.size() < call DataQueue.maxSize()) {
			
			packet = call DataPool.get();
			if(packet == NULL){ //no space available
				return; 
			}
			memcpy(packet,payload,len);
			
			call DataQueue.enqueue(packet);
			call SizeQueue.enqueue(len);
			
			call Forwarding.runForwarding();	
		}
	}

	event message_t * LinkReceive.receive(message_t * msg, void * payload, uint8_t len) {

		generalFrame_t * frame = (generalFrame_t * ) payload;
		
		if(frame->type == DATA_TYPE) {
			stats.packetsRxData++;
			queueForwardingPacket(msg, payload, len);
		}
		else if(frame->type == ROUTING_TYPE) {
			stats.packetsRxDiscovery++;
			signal RoutingEvent.receive(msg, payload, len);
			return msg;
		}
		else {}
		
		return msg;
	}

	command error_t Forwarding.send(void * payload, uint8_t len) {
		payloadFrame_t frame;
	
		dataPacket_t dataPacket; 
		dataPacket_t * d= call DataPool.get();
		
		//Priority to its own packets
		if(d == NULL){
			d = call DataQueue.head();
			call DataQueue.dequeue();
			call DataPool.put(d);
		
			d = call DataPool.get();
		}
		
		dataPacket.generalFrame.networkID = HBTP_TYPE;
		dataPacket.generalFrame.source = TOS_NODE_ID;
		dataPacket.generalFrame.type = DATA_TYPE;
		dataPacket.generalFrame.length = (uint8_t) sizeof(dataPacket_t) - DATA_LENGTH + len + 1;

		dataPacket.hop = call Routing.getCurrentHop();
		dataPacket.battery = call Routing.getBattery();
		dataPacket.payloadsNum = 1;
		dataPacket.payloadsSize = len + 1;

		frame.size = len;
		memcpy(frame.packet, payload, len);

		
		memcpy(dataPacket.payload, &frame, dataPacket.payloadsSize);
		memcpy(d,&dataPacket,dataPacket.generalFrame.length);
		
		call SizeQueue.enqueue(dataPacket.generalFrame.length);
		call DataQueue.enqueue(d);
	
		call Forwarding.runForwarding();
		return SUCCESS;
	}

	command error_t Forwarding.runForwarding() {
		if( ! activeForwardTask) {
			activeForwardTask = TRUE;
			post forwardTask();
		}
		return SUCCESS;
	}
}